#! /usr/bin/env python

'''


'''
import sys
sys.path.append("/home/galm/ms_01/src/command_receiver/scripts/")
from queryControllerImpl import QueryControllerImpl
from pynput import keyboard



def on_press(key):
    #当且仅当主线程进入读取状态时，该线程才接手运行
    if(controller.readFlag):
        global formerHash
        isValid = False
        curHash = 0 #当前状态的hash值
        keyChar = '0'
        try:
            keyChar = key.char
        except AttributeError:
            keyChar = '-'
            '''
                w底盘向前
                s底盘向后
                a底盘左转
                d底盘右转
                q向左平移
                e向右平移
                i机械臂向上
                k机械臂向下
                j机械臂向左
                l机械臂向右
                b射击
            '''
        if(keyChar == 'w'):
            controller.carVel[0] = 1
            isValid = True
        elif(keyChar == 's'):
            controller.carVel[0] = -1
            isValid = True
        #类似于hash值，对于每一位都给定不同的权重，计算当前状态映射的数
        curHash += controller.carVel[0]*11 
        if(keyChar == 'a'):
            controller.carVel[1] = -1
            isValid = True
        elif(keyChar == 'd'):
            controller.carVel[1] = 1
            isValid = True
        if(keyChar == 'q'):
            controller.carVel[1] = -2
            isValid = True
        elif(keyChar == 'e'):
            controller.carVel[1] = 2
            isValid = True
        curHash += controller.carVel[1]*10
        if(keyChar == 'i'):
            controller.armPos[0] = 1
            isValid = True
        elif(keyChar == 'k'):
            controller.armPos[0] = -1
            isValid = True
        curHash += controller.armPos[0]*50
        if(keyChar == 'j'):
            controller.armPos[1] = -1
            isValid = True
        elif(keyChar == 'l'):
            controller.armPos[1] = 1
            isValid = True
        curHash += controller.armPos[1]*100
        if(keyChar == 'b' and controller.state == "fcs"):
            controller.shoot = True
            isValid = True
            curHash += 200
        print(curHash)
        isNew = True
        if(curHash == formerHash):
            #如果当前hash数值和之前的hash数值相等
            #那么就证明数据不是新的
            isNew = False
        #让当前的hash数变为上一个hash数
        print(formerHash)
        formerHash = curHash
        if(isValid and isNew):
            controller.publish()
            #只有数据有效而且是新数据的时候，才会发布信息
        


def on_release(key):
    if(controller.readFlag):
        global formerHash
        isValid = False  #检测数据是否有效
        keyChar = '0'
        try:
            keyChar = key.char
        except AttributeError:
            keyChar = '-'
            '''
                w底盘向前
                s底盘向后
                a底盘左转
                d底盘右转
                i机械臂向上
                k机械臂向下
                j机械臂向左
                l机械臂向右
                b射击
            '''
            #如果是松开了这些按键，那么就将对应的速度设置为0
        if(keyChar == 'w' or keyChar == 's'):
            controller.carVel[0] = 0
            isValid = True
        if(keyChar == 'a' or keyChar == 'd' or keyChar == 'q' or keyChar == 'e'):
            controller.carVel[1] = 0
            isValid = True
        if(keyChar == 'i' or keyChar == 'k'):
            controller.armPos[0] = 0
            isValid = True
        if(keyChar == 'j' or keyChar == 'l'):
            controller.armPos[1] = 0
            isValid = True
        if(keyChar == 'b'):
            controller.shoot = False
            isValid = True
        if(keyChar == 'p'):
            controller.readFlag = False
            return
        if(isValid):
            controller.publish()
            formerHash = -1
        

controller = QueryControllerImpl()
'''
    如何避免重复发出指令
    1.需要在按下按键和抬起按键时检查是否为指定按键，如果不为指定按键，则不发送数据
    设置flag，初始为false，在每个if处检查，如果有对应按键，那么就将该数设为true
    如果flag为true，那么就发送数据
    2.由于如果持续按下按键，数据会持续发送，因此要在on_press函数下实时检测机器人
    状态较上一个状态是否改变，如果有变化，那么就不要发送

'''
formerHash = -1 #用于记录之前的状态


if __name__ == "__main__":
    listener = keyboard.Listener(
        on_press=on_press,
        on_release=on_release)
    listener.start()
    controller.query()
